#pragma once

#include <math.h>
#include <d3dx9.h>

#define PI 3.141597f

namespace D3DMath {
	inline float sqr(float f){return f*f;}
	inline float sqr2(float f){return f < 0 ? -f*f : f*f;}
	inline float pow3(float f){return f*f*f;}

	class Matrix4x4;
	class Quaternion;

	class Vec4 {
	public:
		float x,y,z,w;
		Vec4() {}
		Vec4(float a, float b, float c, float d) {x=a; y=b; z=c; w=d;}
		Vec4 Multiply4D(const Matrix4x4 &m) const;
	};

	class Vec3 {
	public:
		float x,y,z;

		Vec3() { }
		explicit Vec3(float f) {x = y = z = f;}
		Vec3(const float _x, const float _y, const float _z) {
			x=_x; y=_y; z=_z;
		}
		void CopyTo(float *dest) const {
			dest[0] = x;
			dest[1] = y;
			dest[2] = z;
		}
		Vec3 operator + (const Vec3 &other) const {
			return Vec3(x + other.x, y + other.y, z + other.z);
		}
		void operator += (const Vec3 &other) {
			x += other.x; y += other.y; z += other.z;
		}
		Vec3 operator -(const Vec3 &v) const {
			return Vec3(x-v.x,y-v.y,z-v.z);
		}
		void operator -= (const Vec3 &other)
		{
			x-=other.x; y-=other.y; z-=other.z;
		}
		Vec3 operator -() const {
			return Vec3(-x, -y, -z);
		}

		Vec3 operator * (const float f) const {
			return Vec3(x*f, y*f, z*f);
		}
		Vec3 operator / (const float f) const {
			float invf = (1.0f / f);
			return Vec3(x*invf, y*invf, z*invf);
		}
		void operator /= (const float f) {
			*this = *this / f;
		}
		float operator * (const Vec3 &other) const {
			return x*other.x + y*other.y + z*other.z;
		}
		void operator *= (const float f) {
			*this = *this * f;
		}
		Vec3 ScaledBy(const Vec3 &other) const {
			return Vec3(x*other.x, y*other.y, z*other.z);
		}
		Vec3 ScaledByInv(const Vec3 &other) const
		{
			return Vec3(x/other.x, y/other.y, z/other.z);
		}
		Vec3 operator *(const Matrix4x4 &m) const;
		void operator *=(const Matrix4x4 &m)
		{
			*this = *this * m;
		}
		Vec4 Multiply4D(const Matrix4x4 &m) const;
		Vec3 RotatedBy(const Matrix4x4 &m) const;

		Vec3 operator %(const Vec3 &v) const {
			return Vec3(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
		}  
		float Length2() const {
			return x*x + y*y + z*z;
		}
		float Length() const {
			return sqrtf(Length2());
		}
		void SetLength(const float l) {
			(*this) *= l/Length();
		}
		Vec3 WithLength(const float l) const {
			return (*this) * l / Length();
		}
		float Distance2To(Vec3 &other) {
			return Vec3(other - (*this)).Length2();
		}
		Vec3 Normalized() const {
			return (*this) / Length();
		}
		float Normalize() { //returns the previous length, is often useful
			float len = Length();
			(*this) = (*this)/len;
			return len;
		}
		float &operator [] (int i) {
			return *((&x) + i);
		}
		const float operator [] (const int i) const	{
			return *((&x) + i);
		}
		bool operator == (const Vec3 &other) const {
			if (x==other.x && y==other.y && z==other.z)
				return true;
			else 
				return false;
		}
		Vec3 Lerp(const Vec3 &other, const float t) const {
			return (*this)*(1.0f - t) + other*t;
		}
		void SetZero() {
			memset((void *)this, 0, sizeof(float)*3);
		}
		operator D3DXVECTOR3*() const {
			return (D3DXVECTOR3*)(this);
		}
	};

	inline Vec3 operator * (const float f, const Vec3 &v) {return v * f;}

	class Matrix4x4 {
	public:
		float xx, xy, xz, xw;
		float yx, yy, yz, yw;
		float zx, zy, zz, zw;
		float wx, wy, wz, ww;

		// These are slightly hacky. Still useful :)
		Vec3 &right() {return *(Vec3 *)&xx;}
		Vec3 &up()    {return *(Vec3 *)&yx;}
		Vec3 &front() {return *(Vec3 *)&zx;}
		Vec3 &move()  {return *(Vec3 *)&wx;}

		const float *operator[](int i) const {
			return ((const float *)this) + i*4;
		}
		float *operator[](int i) {
			return ((float *)this) + i*4;
		}
		Matrix4x4 operator * (const Matrix4x4 &other) const;
		void operator *= (const Matrix4x4 &other) {
			*this = *this * other;
		}
		void empty() {
			memset(this,0,16*sizeof(float));
		}
		void SetScaling(const float f) {
			empty();
			xx=yy=zz=f; ww=1.0f;
		}
		void SetScaling(const Vec3 f) {
			empty();
			xx=f.x;
			yy=f.y;
			zz=f.z; 
			ww=1.0f;
		}

		void SetIdentity() {
			SetScaling(1.0f);
		}
		void SetTranslation(const Vec3 &trans) {
			SetIdentity();
			move() = trans;
		}
		Matrix4x4 inverse() const;
		Matrix4x4 simpleInverse() const;
		Matrix4x4 transpose() const;

		void SetRotationX(const float a) {
			empty();
			float c=cosf(a);
			float s=sinf(a);
			xx = 1.0f;
			yy =  c;      yz = s;    
			zy = -s;      zz = c;    
			ww = 1.0f;
		}
		void SetRotationY(const float a)   {
			empty();
			float c=cosf(a);
			float s=sinf(a);
			xx = c;                   xz = -s;   
			yy =  1.0f;               
			zx = s;                   zz = c  ;  
			ww = 1.0f;
		}
		void SetRotationZ(const float a)   {
			empty();
			float c=cosf(a);
			float s=sinf(a);
			xx = c;    xy = s;   
			yx = -s;   yy = c;               
			zz = 1.0f; 
			ww = 1.0f;
		}

		void SetRotation(float x, float y, float z);
		void SetProjection(const float near_plane, const float far_plane,
			               const float fov_horiz, const float aspect = 0.75f);
		void SetProjectionInf(const float near_plane, const float fov_horiz, const float aspect = 0.75f);
		void SetShadow(float Lx, float Ly, float Lz, float Lw) {
			float Pa = 0;
			float Pb = 1;
			float Pc = 0;
			float Pd = 0;
			//P = normalize(Plane);
			float d = (Pa*Lx + Pb*Ly + Pc*Lz + Pd*Lw);

			xx=Pa * Lx + d;  xy=Pa * Ly;     xz=Pa * Lz;     xw=Pa * Lw;
			yx=Pb * Lx;      yy=Pb * Ly + d; yz=Pb * Lz;     yw=Pb * Lw;
			zx=Pc * Lx;      zy=Pc * Ly;     zz=Pc * Lz + d; zw=Pc * Lw; 
			wx=Pd * Lx;      wy=Pd * Ly;     wz=Pd * Lz;     ww=Pd * Lw + d;
		}

		void SetViewLookAt(const Vec3 &from, const Vec3 &at, const Vec3 &worldup);
		void stabilizeOrtho() {
			// Gramm-Schmidt, sort of (not really)
			front().Normalize();
			right().Normalize();
			up() = front() % right();
			right() = up() % front();
		}
		static Matrix4x4 fromPRS(const Vec3 &position, const Quaternion &normal, const Vec3 &scale);
	};


	class Quaternion
	{
	public:
		float x,y,z,w;

		Quaternion() { }
		Quaternion(const float _x, const float _y, const float _z, const float _w) {
			x=_x; y=_y; z=_z; w=_w;
		}
		void SetIdentity() {
			x=y=z=0; w=1.0f;
		}
		void SetXRotation(const float r) { w = cosf(r / 2); x = sinf(r / 2); y = z = 0; }
		void SetYRotation(const float r) { w = cosf(r / 2); y = sinf(r / 2); x = z = 0; }
		void SetZRotation(const float r) { w = cosf(r / 2); z = sinf(r / 2); x = y = 0; }

		Matrix4x4 toMatrix() const;
		static Quaternion FromMatrix(const Matrix4x4 &m);
		Quaternion operator *(const Quaternion &q) const {
			return Quaternion(
				(w * q.w) - (x * q.x) - (y * q.y) - (z * q.z),
				(w * q.x) + (x * q.w) + (y * q.z) - (z * q.y),
				(w * q.y) + (y * q.w) + (z * q.x) - (x * q.z),
				(w * q.z) + (z * q.w) + (x * q.y) - (y * q.x)
				);
		}
		Quaternion operator -() const {
			return Quaternion(-x,-y,-z,-w);
		}
		void SetRotation(const Vec3 &axis, float angle) {
			Vec3 axis_n = axis.Normalized();
			angle *= .5f;
			float sine = sinf(angle);
			w = cosf(angle);
			x = sine * axis_n.x;
			y = sine * axis_n.y;
			z = sine * axis_n.z;
		}
		void ToAxisAngle(Vec3 &v, float &angle) {
			Normalize();
			if (w==1.0f && x==0.0f && y==0.0f && z==0.0f)
			{
				v = Vec3(0,1,0);
				angle = 0.0f;
				return;
			}
			float cos_a = w;
			angle = acosf(cos_a) * 2;
			float sin_a = sqrtf(1.0f - cos_a * cos_a);
			if (fabsf(sin_a) < 0.00005f) sin_a = 1;
			float inv_sin_a = 1.0f/sin_a;
			v.x = x * inv_sin_a;
			v.y = y * inv_sin_a;
			v.z = z * inv_sin_a;
		}
		enum {
			QUAT_SHORT,
			QUAT_LONG,
			QUAT_CW,
			QUAT_CCW
		};
		Quaternion Slerp(const Quaternion &to, const float a) const;
		Quaternion Multiply(const Quaternion &q) const;
		float &operator [] (int i) {
			return *((&x) + i);
		}
		const float operator [] (int i) const {
			return *((&x) + i);
		}
		//not sure about this, maybe mag is supposed to sqrt
		float Magnitude() const {
			return x*x + y*y + z*z + w*w;
		}
		void Normalize() {
			float f = 1.0f / sqrtf(Magnitude());
			x*=f; y*=f; z*=f; w*=f;
		}
	};

	class AABBox {
	public:
		Vec3 min;
		Vec3 max;
	};

	/*
	class BSphere
	{
	public:
	Vec3 center;
	float radius;
	};

	inline BSphere joinBSpheres(BSphere &b1, BSphere &b2)
	{
	BSphere s;
	s.center = (b1.center+b2.center)/
	}*/
}